/*
 * BSpline_6.c
 *
 *  Created on: Oct 15, 2011
 *      Author: yifili09
 */

/*
 * bsp.c
 *
 *  Created on: Oct 13, 2011
 *      Author: Yifei
 */

#include "stdio.h"
#include "Point/point.h"
#include <complex.h>	//http://man.he.net/man3/cexpf
#include <math.h>

//#define DIVISON 20
//float splinex[DIVISON - 1];
//float spliney[DIVISON - 1];

point *pStart, *pEnd;
//void bsp_6(point p1, point p2, point p3, point p4, int divison);

#define KV 130
#define L 0.2




//void newSpline(point *p2, point *p5, int n);
//void bspline(float _Complex Z, int n);

int maincc() {

	printf("HelloWorld %f\n",sqrt(-1));


	_Complex float x = 5.11;
	//float _Complex y = 5.11 + 1.11 * I;
	//float y = 5.11 + 1.11*(sqrt(-1));

	//printf("%g + %g\n", creal(x), cimag(x));
	/*
	 point p[5];
	 p1 = pointAlloc(1.0, 1.0);

	 p2 = pointAlloc(15.0, 20.0);
	 p3 = pointAlloc(3.0, 3.0);
	 p4 = pointAlloc(5.8, 5.0);

	 p[1] = *p1;
	 p[2] = *p2;
	 p[3] = *p3;
	 p[4] = *p4;
	 */

	pStart = pointAlloc(1, 1, 45); // 1 + 1*I with 45 Degree
	pEnd = pointAlloc(4, 7, -45); // 4 + 7*I with -45 Degree

	point pointArray[2]; // Just Check the Array
	pointArray[0] = *pStart;
	pointArray[1] = *pEnd;
	int var;
	for (var = 0; var < 2; ++var) {
		printf("The point is %d: (%f,%f) \n", var, pointArray[var].x,
				pointArray[var].y);
	}

	//newSpline(pStart, pEnd, 10); //Create the 6 CheckPoint

	//int n = DIVISON;
	//bsp(*p1, *p2, *p3, *p4, n);
	/*

	 for (var = 0; var < DIVISON; ++var) {
	 printf("Spline: (%f,%f)\n", splinex[var], spliney[var]);
	 }
	 */


	free(pStart);
	free(pEnd);
	return 0;
}

void ORG_newSpline(point *p2, point *p5, int n) {

	/**** Check the pStart and pEnd **********************/
	printf("The Detail of p2:%f,%f,%f\n", p2->x, p2->y, p2->Theta);
	printf("The Detail of p5:%f,%f,%f\n", p5->x, p5->y, p5->Theta);
	/****************************************************/

	float _Complex zA = p2->x + p2->y * I;
	float _Complex zB = p5->x + p5->y * I;
	/****Check the Complex Number of Point****************/
	printf("The zA :%g + %g*I\nThe zB :%g + %g*I\n", crealf(zA), cimagf(zA),
			crealf(zB), cimagf(zB));
	//printf("The zA angle:%f \n",cargf(zA));
	/****************************************************/

	//float r = sqrtf((powf((p5->x - p2->x), 2) + powf((p5->y - p2->y), 2))) / 3;
	float r = cabsf(zB - zA) / 3.0;
	/****Check the number of r ***************************/
	printf("r: %f\n", r); //Check the r
	//float rf = cabsf(zB-zA)/3;
	//printf("rf: %f\n", rf);
	/****************************************************/

	float _Complex Z[6];
	point ZP[6];

	/****Check the sub of Complex ***********************/
	double _Complex zC = zB - zA;
	printf("The Sub zB-zA: %g %g\n", crealf(zC), cimagf(zC));

	zC = 3 * zC;
	printf("The Times zB-zA: %g %g\n", crealf(zC), cimagf(zC));
	/****************************************************/

	// 3 Control Point for pStart
	Z[0] = zA - r * cexpf(I * p2->Theta);
	Z[1] = zA;
	Z[2] = zA + r * cexpf(I * p2->Theta);


	// 3 Control Point for pEnd
	Z[3] = zB - r * cexpf(I * p5->Theta);
	Z[4] = zB;
	Z[5] = zB + r * cexpf(I * p5->Theta);

	/****Check the Z point Array *************************/
	int var = 0;
	for (var = 0; var < 6; ++var) {
		printf("The Z[%d] is %g %gI\n", var, crealf(Z[var]), cimagf(Z[var]));
	}
	/****************************************************/

	/**** bspline ***************************************/
	float _Complex tempA[4];
	float _Complex splineComplex[31];
	int count = 0;
	float t = 0;
	int i = 0;
	splineComplex[0] = zA;

	for (var = 0; var < 3; ++var) {

		tempA[0] = (-Z[var] + 3 * Z[var + 1] - 3 * Z[var + 2] + Z[var + 3])
				/ 6.0;
		tempA[1] = (3 * Z[var] + -6 * Z[var + 1] + 3 * Z[var + 2]) / 6.0;
		tempA[2] = (-3 * Z[var] + 3 * Z[var + 2]) / 6.0;
		tempA[3] = (Z[var] + 4 * Z[var + 1] + Z[var + 2]) / 6.0;

		for (i = 1; i < n + 1; i++) {
			t = (float) i / n;
			count = var * n + i;
			splineComplex[count] = (tempA[2] + t * (tempA[1] + t * tempA[0]))
					* t + tempA[3];
			/***** Check the splineComplex ********/
			printf("The splineComplex[%d]: %g + %gI\n", count,
					crealf(splineComplex[count]), cimagf(splineComplex[count]));
			/*************************************/
		}
	}

	for (var = 0; var < 31; var++) {
		printf("The splineComplex[%d]: %g + %gI\n", var,
				crealf(splineComplex[var]), cimagf(splineComplex[var]));
	}

	/****************************************************/

	/**** Control Part: Calculate the U and V ************/
	float _Complex dZ[30];
	float v[30];
	float th[30];
	float u[29];

	for (var = 0; var < 30; ++var) {
		dZ[var] = splineComplex[var + 1] - splineComplex[var];
		v[var] = cabsf(dZ[var]);
		th[var] = cargf(dZ[var]);
		/****************************************************/
		printf("The v[%d] is %f\n", var, v[var]);
		printf("The th[%d] is %f\n", var, th[var]);
		/****************************************************/
	}

	for (var = 0; var < 29; ++var) {		// u ..[0...29]

		u[var] = th[var + 1] - th[var];
		if (u[var] > PI) {
			u[var] = u[var] - 2 * PI;
		} else if (u[var] < -PI) {
			u[var] = u[var] + 2 * PI;
		}

		/****************************************************/
		printf("The u[%d] is %f\n", var, u[var]);
		/****************************************************/
	}

	/****************************************************/


	/**** Calculating the  WR and WL**********************/
	int wR[30];
	int wL[30];

	for (var = 0; var < 29; ++var) {
		wR[var] = (int)(KV*(v[var] + (float)(L/2.0)*u[var]) )+ 128;
		wL[var] = (int)(KV*(v[var] - (float)(L/2.0)*u[var]) )+ 128;

		/****************************************************/
		printf("The Speed Set[%d] is :(%d,%d)\n",var,wL[var],wR[var]);
		//printf("The Speed Set[%d] is :(%f,%f)\n",var,wL[var],wR[var]);

		/****************************************************/

	}

/*
		for (var = 0; var < 29; ++var) {
				wR[var] = (int)(KV*(v[var] + (float)(L/2.0)*u[var]) )+ 128;
				wL[var] = (int)(KV*(v[var] - (float)(L/2.0)*u[var]) )+ 128;

				int TimeTick = 0;
				//setspeed()
				while(TimeTick <T);


			}


*/
	/****************************************************/

}

//For 4 Controller
/*
 void bsp_6(point p2, point p5, int divison) {
 double a[5];
 double b[5];
 a[0] = (-p1.x + 3 * p2.x - 3 * p3.x + p4.x) / 6.0;
 a[1] = (3 * p1.x - 6 * p2.x + 3 * p3.x) / 6.0;
 a[2] = (-3 * p1.x + 3 * p3.x) / 6.0;
 a[3] = (p1.x + 4 * p2.x + p3.x) / 6.0;
 b[0] = (-p1.y + 3 * p2.y - 3 * p3.y + p4.y) / 6.0;
 b[1] = (3 * p1.y - 6 * p2.y + 3 * p3.y) / 6.0;
 b[2] = (-3 * p1.y + 3 * p3.y) / 6.0;
 b[3] = (p1.y + 4 * p2.y + p3.y) / 6.0;
 splinex[0] = a[3];
 spliney[0] = b[3];
 int i;
 //Choose 4 controller for the calculation
 for (i = 1; i <= divison - 1; i++) {
 float t = i / divison;
 splinex[i] = (a[2] + t * (a[1] + t * a[0])) * t + a[3];
 spliney[i] = (b[2] + t * (b[1] + t * b[0])) * t + b[3];
 }

 }*/

